import ctypes
import logging
import math
import os
import time
import socket
import logging
import serial

logging.basicConfig(filename='erweima.log', level=logging.DEBUG, format='%(asctime)s %(levelname)s: %(message)s')


class POSE(ctypes.Structure):
    _fields_ = [("px", ctypes.c_float),
                ("py", ctypes.c_float),
                ("pz", ctypes.c_float),
                ("rx", ctypes.c_float),
                ("ry", ctypes.c_float),
                ("rz", ctypes.c_float)]


class SockCom():



    """
    sock的建立 发送 接受
    """

    def __init__(self,ip,port):

        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.setblocking(True)  #设置阻塞
        # 连接到目标主机和端口
        server_address = (ip, port)
        logging.info('connecting to %s port %s' % server_address)
        self.tag = self.sock.connect_ex(server_address)

        self.is_connect()

    def is_connect(self):

        if self.tag: #非0显示连接失败
            logging.error(f'socket连接失败:{self.tag}')

        else:
            logging.info(f'socket连接成功')



    def send(self,character):


        """
        character:给照相机发送的指令 发送指令 paizhao就会拍照
        """

        self.message = character
        logging.info('sending "%s"' % self.message)
        self.sock.sendall(self.message.encode())

    def recv(self):
        """

        :return: material,px,py  代表当前识别出的颜色的数字，物品在相机坐标系x轴的位置，物品在相机坐标系y轴的位置

        """
        # 接收返回值
        logging.info('开始拍照')
        data = self.sock.recv(1024)
        str2 = data.decode()

        while str2 == "00":
            logging.info(f'当前相机返回的信息：00')

            self.sock.send(self.message.encode('utf-8'))
            data = self.sock.recv(1024)
            str2 = data.decode()
            time.sleep(2)

        logging.info(f'当前相机返回的信息：{str2}')

        try:
            result = str2.split(',')

            if len(result) > 1:
                material = result[0]
                px = float(result[1])/1000 - 0.26024 #吸取基准点X
                py = float(result[2])/1000 + 0.145 #吸取基准点Y

                # px = float(result[1])/1000 - 0.26
                # py = float(result[2])/1000 + 0.15



                rz = float(result[3])* math.pi /180

                return [material, px, py, rz]

            else:

                return result[0]

        except Exception as e:
            logging.error(f'拍照处理结果出错：{e}')


    def close(self):

        self.sock.close()


class SerCom():
    """
    串口建立，发送 接受
    """


    def __init__(self,path,baud_rate):

        #串口设备的名称，例如 /dev/ttyUSB0（Linux 系统）或 COM1（Windows 系统）
        #波特率，即数据传输速率


        try:
            # 打开串口
            self.ser = serial.Serial(path, baud_rate, timeout=1, parity=serial.PARITY_NONE, bytesize=serial.EIGHTBITS,
                                     stopbits=serial.STOPBITS_ONE)
        except serial.SerialException as e:
            # 捕获异常并打印错误原因
            logging.info('串口连接失败！原因为：', e)
        else:
            # 连接成功，打印串口信息
            print('串口连接成功！串口信息：', self.ser)

    def is_connect(self):
        """
        判断ser是否连接成功
        """

        if self.ser.is_open:
            print('串口连接成功！')
            logging.info('串口建立连接')
        else:
            logging.info('串口连接失败')


    def send(self):

        data = bytes.fromhex('01 06 60 02 00 10 37 C6')
        # 发送数据
        self.ser.write(data)

    def read(self):

        ret = self.ser.read()

    def close(self):

        self.ser.close()


class Arm:
    def __init__(self, ip=''):
        self.ip = ip
        CUR_PATH = os.path.dirname(os.path.realpath(__file__))
        dllPath = os.path.join(CUR_PATH, "RM_Base.dll")
        self.pDll = ctypes.cdll.LoadLibrary(dllPath)
        #   API 初始化


        #   连接机械臂
        byteIP = bytes(self.ip, "gbk")
        self.nSocket = self.pDll.Arm_Socket_Start(byteIP, 8080, 2000)
        logging.info(f'连接机械臂完毕')
        self.ret = self.pDll.Arm_Socket_State(self.nSocket)
        logging.info(f'连接状态：{self.ret}')

        ver = ctypes.c_char_p()
        self.pDll.API_Version.restype = ctypes.c_char_p
        ver = self.pDll.API_Version()
        print(ver)



    def set_io(self,io,num,state,block):

        tag = self.pDll.Set_IO_State(self.nSocket,io,num,state,block)
        logging.info(f'Set_IO_State执行的结果：{tag}')

        return tag

    def io_judgment(self, label):
        """
        判断中央导轨是否到达了两端传感器 如果接近
        Get_IO_Input 查询所有数字和模拟IO的输入状态
        ArmSocket socket句柄
        DI_state 数字IO输入通道1~3状态数组地址，1-高，0-低
        AI_voltage 模拟IO输入通道1~4输入电压数组
        return 0-成功，失败返回:错误码, rm_define.h查询.
        """

        DI_state = (ctypes.c_byte * 3)(1, 1, 1)  # 假设 DI_state 的长度为 3，初始值为 1
        AI_voltage = (ctypes.c_float * 4)(1.0, 1.0, 1.0, 1.0)  # 假设 AI_voltage 的长度为 4，初始值为 1.0

        logging.info(f'DI_state:{DI_state[0]}')
        print(f'DI_state:{DI_state[0]}')

        if label:
            while DI_state[0] == 1:
                time.sleep(0.5)
                ret = self.pDll.Get_IO_Input(self.nSocket, DI_state, AI_voltage)
                print('DI_state', DI_state[0])
                logging.info(f'Get_IO_Input:{ret}')
                print(f'Get_IO_Input:{ret}')

                if ret == 0 and DI_state[0] == 0:
                    print(f"Get_IO_Input 到达地方:{ret}")
                    return
                elif ret != 0:
                    print(f"Get_IO_Input 调用失败，返回值为 {ret}")

        else:
            print('label', label)
            while DI_state[2] == 1:  # 如果是中央导轨从右往左移动  左机械臂 3通道获取 中央导轨的输入信号  ，操作中央导轨都是右机械臂
                time.sleep(0.5)
                ret = self.pDll.Get_IO_Input(self.nSocket, DI_state, AI_voltage)
                print('DI_state', DI_state[2])
                logging.info(f'Get_IO_Input:{ret}')
                print(f'Get_IO_Input:{ret}')

                if ret == 0 and DI_state[2] == 0:
                    print(f"Get_IO_Input 到达地方:{ret}")
                    return
                elif ret != 0:
                    print(f"Get_IO_Input 调用失败，返回值为 {ret}")

    def Rot2_init(self):
        """
        夹爪机械臂初始化
        1.末端电压输出24V
        2.打开夹爪
        3.ROT2机械臂以及导轨回到初始位置
        4.旋转仓储旋转90°
        :return:
        """
        #    初始化

        # tag = self.pDll.Set_Tool_Voltage(self.nSocket, 3, 1)
        # logging.info(f'设置末端电压：{tag}')
        #
        # tag = self.pDll.Set_Gripper_Release(self.nSocket, 500, 1)
        # logging.info(f'夹爪张开：{tag}')
        #
        # self.pDll.Set_Lift_Height.argtypes = (ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_bool)
        # self.pDll.Set_Lift_Height.restype = ctypes.c_int
        # tag = self.pDll.Set_Lift_Height(self.nSocket, 230, 50, 1)
        # logging.info(f'导轨初始位置：{tag}')

        float_joint = ctypes.c_float * 6
        joint1 = float_joint(*[0, -4.102, 0, -0.335, 90, -51.525])
        self.pDll.Movej_Cmd.argtypes = (ctypes.c_int, ctypes.c_float * 6, ctypes.c_byte, ctypes.c_float, ctypes.c_bool)
        self.pDll.Movej_Cmd.restype = ctypes.c_int
        tag = self.pDll.Movej_Cmd(self.nSocket, joint1, 20, 0, 1)
        logging.info(f'运行至初始位置：{tag}')

        return tag

    def Rot2_trans(self):
        """
        夹爪机械臂将物料盘从旋转仓储运输到中央导轨
        :return:
        """
        #   运输物料盘
        self.pDll.Movej_Cmd.argtypes = (ctypes.c_int, ctypes.c_float * 6, ctypes.c_byte, ctypes.c_float, ctypes.c_bool)
        self.pDll.Movej_Cmd.restype = ctypes.c_int
        float_joint = ctypes.c_float * 6
        joint1 = float_joint(*[96.009, -40.432, -38.951, 0.515, -101.218, -82.26])
        tag = self.pDll.Movej_Cmd(self.nSocket, joint1, 20, 0, 1)
        logging.info(f'运行至物料盘上方：{tag}')

        po1 = POSE()
        po1.px, po1.py, po1.pz, po1.rx, po1.ry, po1.rz = [-0.040369, 0.368299, 0.29172, -3.13, -0.007, 3.112]
        self.pDll.Movel_Cmd.argtypes = (ctypes.c_int, POSE, ctypes.c_byte, ctypes.c_float, ctypes.c_int)
        self.pDll.Movel_Cmd.restype = ctypes.c_int
        tag = self.pDll.Movel_Cmd(self.nSocket, po1, 20, 0, 1)
        logging.info(f'准备夹取：{tag}')

        tag = self.pDll.Set_Gripper_Pick(self.nSocket, 750, 750, 1)
        logging.info(f'夹取物料盘:{tag}')
        time.sleep(1)

        po1.pz += 0.05
        tag = self.pDll.Movel_Cmd(self.nSocket, po1, 20, 0, 1)
        logging.info(f'离开仓储平台:{tag}')

        joint2 = float_joint(*[132.565, -16.04, -49.221, 1.529, -117.154, -46.861])
        tag = self.pDll.Movej_Cmd(self.nSocket, joint2, 20, 0, 1)
        logging.info(f'Movej_Cmd：{tag}')

        po2 = POSE()
        po2.px, po2.py, po2.pz, po2.rx, po2.ry, po2.rz = -0.17394, 0.1803, 0.31504, -3.094, 0.01, 3.144
        tag = self.pDll.Movel_Cmd(self.nSocket, po2, 20, 0, 1)
        logging.info(f'Movel_Cmd：{tag}')

        tag = self.pDll.Set_Gripper_Release(self.nSocket, 500, 1)
        logging.info(f'物料盘放置到中央导轨：{tag}')
        time.sleep(1)

        po3 = POSE()
        po3.px, po3.py, po3.pz, po3.rx, po3.ry, po3.rz = -0.175259, 0.185759, 0.430579, -3.094, 0.01, 3.144
        tag = self.pDll.Movel_Cmd(self.nSocket, po3, 20, 0, 1)
        logging.info(f'机械臂夹爪退回：{tag}')

        joint3 = float_joint(*[131.682, -4.102, -63.948, -0.335, -110.513, -51.525])
        tag = self.pDll.Movej_Cmd(self.nSocket, joint3, 20, 0, 1)
        logging.info(f'运行至初始位置：{tag}')



    def Rot1_run(self):
        """
        视觉机械臂准备拍照
        :return:
        """
        self.pDll.Movej_Cmd.argtypes = (ctypes.c_int, ctypes.c_float * 6, ctypes.c_byte, ctypes.c_float, ctypes.c_bool)
        self.pDll.Movej_Cmd.restype = ctypes.c_int
        float_joint = ctypes.c_float * 6
        joint1 = float_joint(*[21.528, -8.161, 93.427, -0.034, 94.816, -36.566])
        tag = self.pDll.Movej_Cmd(self.nSocket, joint1, 20, 0, 1)
        logging.info(f'Rot1运行至二维码识别位置：{tag}')

    def close(self):
        self.pDll.Arm_Socket_Close(self.nSocket)

def second_rail(ROT1,ROT2,label):
    """
    中央导轨移动到两端处 高电压靠近右臂 低电压靠近左臂
    **中央导轨从第二个机械臂移动到第一个机械臂处 此时 使用者给它一个低电平（输出），如果它到达第一个机械臂处，此时中央导轨输入一个低电平

    输入和输出都是相对于  使用者而言**
    """
    if label:
        logging.info(f'中央导轨向右臂移动：{label}')
        print(f'中央导轨向右臂移动：{label}')

        ROT1.set_io(0, 1, True, 1)  # 中央导轨的移动准备拍摄二维码，如果到达右臂处，数字的输入状态变为0 chu io,num,state,block
        ROT1.io_judgment(label)  # 是否在整个流程开始时 让中央导轨初始化位于第一个机械臂处

    else:
        logging.info(f'中央导轨向左臂移动：{label}')
        print(f'中央导轨向左臂移动：{label}')

        ROT1.central_rail(0, 1, False, 1)  # 中央导轨的移动初始化，如果到达初始位置，数字的输入状态变为0 io,num,state,block
        time.sleep(1)
        ROT2.io_judgment(label)  # 是否在整个流程开始时 让中央导轨初始化位于第一个机械臂处


def main(ROT1, ROT2, ser, sock):
    ROT2.Rot2_init()
    second_rail(ROT1, ROT2, 1)

    ser.send()
    logging.info(f'旋转仓储旋转90°')
    ROT2.Rot2_trans()
    second_rail(ROT1, ROT2, 0)
    ROT1.Rot1_run()
    sock.send("paizhao")
    material = sock.recv()
    logging.info(f'二维码颜色：{material}')


if __name__ == "__main__":
    # sock = SockCom('192.168.1.10', 6000)
    CUR_PATH = os.path.dirname(os.path.realpath(__file__))
    dllPath = os.path.join(CUR_PATH, "RM_Base.dll")
    pDll = ctypes.cdll.LoadLibrary(dllPath)
    pDll.RM_API_Init(65, 0)
    ROT1 = Arm(ip="192.168.1.19")
    ROT2 = Arm(ip='192.168.1.18')
    # ser = SerCom('COM1', 38400)
    ROT1.Rot2_init()
    ROT2.Rot2_init()
    # main(ROT1, ROT2, ser, sock)

    ROT1.close()
    ROT2.close()
    # ser.close()
    # sock.close()
